// --------------------------------------------------------------------------------------------------------------------
// <copyright file="URKinematicConfigurations.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.UniversalRobots
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;

    using ShoNS.Array;

    /// <summary>
    /// A class that includes functions to create the kinematic chain for a UR arm.
    /// </summary>
    public class URKinematicConfigurations
    {
        /// <summary>
        /// Create the 6DOF kinematic chain of UR5 robot arm
        /// </summary>
        /// <param name="endEffectorOffsetInLastJoint">Offset of the end effector in last joint, default is zero offset</param>
        /// <returns>A kinematic chain</returns>
        public static KinematicChain CreateUR5KinematicChain(Matrix4 endEffectorOffsetInLastJoint = null)
        {
            int numJoints = 6;

            // create the joint list
            Joint[] jointList = new Joint[numJoints];
            jointList[0] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, 0, 0.089159), 0, -MathConstants.TwoPI, MathConstants.TwoPI);
            jointList[1] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0.10915), 0, -MathConstants.TwoPI, MathConstants.TwoPI);
            jointList[2] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, -0.425, 0), 0, -MathConstants.TwoPI, MathConstants.TwoPI);
            jointList[3] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(0, -0.39225, 0), 0, -MathConstants.TwoPI, MathConstants.TwoPI);
            jointList[4] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(MathConstants.PIOverTwo, 0, 0.09465), 0, -MathConstants.TwoPI, MathConstants.TwoPI);
            jointList[5] = new Joint(DHParameter.CreateDHParameterForRevoluteJoint(-MathConstants.PIOverTwo, 0, 0.0823), 0, -5.24, 5.24);

            // create the collision model for each link
            // The collision model is a union of spheres. These spheres are selected such
            // that the surface of each link is covered by these spheres.
            // link0 is controlled by joint0
            List<GeometryElement3D> link0Elements = new List<GeometryElement3D>();
            SphereElement l0s0 = new SphereElement(new Vector3(0, 0, 0), 0.08);
            link0Elements.Add(l0s0);
            GeometryElementGroup3D link0Group = new GeometryElementGroup3D(link0Elements);

            // link1 is controlled by joint1
            List<GeometryElement3D> link1Elements = new List<GeometryElement3D>();
            SphereElement l1s0 = new SphereElement(new Vector3(0, 0, 0.02), 0.08);
            SphereElement l1s1 = new SphereElement(new Vector3(-0.09, 0, 0.025), 0.06);
            SphereElement l1s2 = new SphereElement(new Vector3(-0.16, 0, 0.025), 0.06);
            SphereElement l1s3 = new SphereElement(new Vector3(-0.23, 0, 0.025), 0.06);
            SphereElement l1s4 = new SphereElement(new Vector3(-0.30, 0, 0.025), 0.06);
            SphereElement l1s5 = new SphereElement(new Vector3(-0.37, 0, 0.025), 0.06);
            SphereElement l1s6 = new SphereElement(new Vector3(-0.425, 0, 0.03), 0.08);
            link1Elements.Add(l1s0);
            link1Elements.Add(l1s1);
            link1Elements.Add(l1s2);
            link1Elements.Add(l1s3);
            link1Elements.Add(l1s4);
            link1Elements.Add(l1s5);
            link1Elements.Add(l1s6);
            GeometryElementGroup3D link1Group = new GeometryElementGroup3D(link1Elements);

            // link2 is controlled by joint2
            List<GeometryElement3D> link2Elements = new List<GeometryElement3D>();
            SphereElement l2s0 = new SphereElement(new Vector3(0, 0, -0.06), 0.06);
            SphereElement l2s1 = new SphereElement(new Vector3(-0.05, 0, -0.0925), 0.05);
            SphereElement l2s2 = new SphereElement(new Vector3(-0.12, 0, -0.0925), 0.05);
            SphereElement l2s3 = new SphereElement(new Vector3(-0.19, 0, -0.0925), 0.05);
            SphereElement l2s4 = new SphereElement(new Vector3(-0.26, 0, -0.0925), 0.05);
            SphereElement l2s5 = new SphereElement(new Vector3(-0.33, 0, -0.0925), 0.05);
            SphereElement l2s6 = new SphereElement(new Vector3(-0.39, 0, -0.1), 0.06);
            link2Elements.Add(l2s0);
            link2Elements.Add(l2s1);
            link2Elements.Add(l2s2);
            link2Elements.Add(l2s3);
            link2Elements.Add(l2s4);
            link2Elements.Add(l2s5);
            link2Elements.Add(l2s6);
            GeometryElementGroup3D link2Group = new GeometryElementGroup3D(link2Elements);

            // link3 is controlled by joint3
            List<GeometryElement3D> link3Elements = new List<GeometryElement3D>();
            SphereElement l3s0 = new SphereElement(new Vector3(0, 0.02, 0), 0.05);
            SphereElement l3s1 = new SphereElement(new Vector3(0, -0.02, 0), 0.05);
            link3Elements.Add(l3s0);
            link3Elements.Add(l3s1);
            GeometryElementGroup3D link3Group = new GeometryElementGroup3D(link3Elements);

            // link4 is controlled by joint4
            List<GeometryElement3D> link4Elements = new List<GeometryElement3D>();
            SphereElement l4s0 = new SphereElement(new Vector3(0, 0.015, 0), 0.045);
            SphereElement l4s1 = new SphereElement(new Vector3(0, -0.03, 0), 0.045);
            link4Elements.Add(l4s0);
            link4Elements.Add(l4s1);
            GeometryElementGroup3D link4Group = new GeometryElementGroup3D(link4Elements);

            // link5 is controlled by joint5
            List<GeometryElement3D> link5Elements = new List<GeometryElement3D>();
            SphereElement l5s0 = new SphereElement(new Vector3(0, 0, -0.02), 0.04);
            link5Elements.Add(l5s0);
            GeometryElementGroup3D link5Group = new GeometryElementGroup3D(link5Elements);

            // putting all together
            // link6 does not have a collision model associated since it is a very small link
            // and is covered by the collision model of link5
            List<GeometryElementGroup3D> armCollisionModel = new List<GeometryElementGroup3D>();
            armCollisionModel.Add(link0Group);
            armCollisionModel.Add(link1Group);
            armCollisionModel.Add(link2Group);
            armCollisionModel.Add(link3Group);
            armCollisionModel.Add(link4Group);
            armCollisionModel.Add(link5Group);

            // the collision model of the end effector
            List<GeometryElement3D> endEffectorCollisionModel = new List<GeometryElement3D>();

            // palm
            SphereElement gripperSeg1 = new SphereElement(new Vector3(0, 0, 0.12), 0.065);
            SphereElement gripperSeg2 = new SphereElement(new Vector3(0, 0, 0.05), 0.045);
            endEffectorCollisionModel.Add(gripperSeg1);
            endEffectorCollisionModel.Add(gripperSeg2);
            GeometryElementGroup3D customGripperCollisionModel = new GeometryElementGroup3D(endEffectorCollisionModel);

            // arm geometry models
            string[] linkNameList = new string[7];
            linkNameList[0] = "base";
            linkNameList[1] = "link0";
            linkNameList[2] = "link1";
            linkNameList[3] = "link2";
            linkNameList[4] = "link3";
            linkNameList[5] = "link4";
            linkNameList[6] = "link5";

            string[] linkGeometryPaths = new string[7];
            linkGeometryPaths[0] = @"Data\RobotModel\UR5\base.off";
            linkGeometryPaths[1] = @"Data\RobotModel\UR5\link0.off";
            linkGeometryPaths[2] = @"Data\RobotModel\UR5\link1.off";
            linkGeometryPaths[3] = @"Data\RobotModel\UR5\link2.off";
            linkGeometryPaths[4] = @"Data\RobotModel\UR5\link3.off";
            linkGeometryPaths[5] = @"Data\RobotModel\UR5\link4.off";
            linkGeometryPaths[6] = @"Data\RobotModel\UR5\link5.off";

            KinematicChain kc = new KinematicChain(jointList, linkNameList, linkGeometryPaths, "CustomGripper", null, armCollisionModel, customGripperCollisionModel);

            // where the EE is w.r.t. the last joint
            Matrix4 endEffectorInLastJoint = (null == endEffectorOffsetInLastJoint) ? HomogenousTransform.CreateFromTranslation(new Vector3(0, 0, 0)) : endEffectorOffsetInLastJoint;
            kc.SetEndEffectorOffsetInLastJoint(endEffectorInLastJoint);

            return kc;
        }

        /// <summary>
        /// Get joint configuration for the home position
        /// </summary>
        /// <returns>Joint configuration for the home position</returns>
        public static DoubleArray GetHomeJointConfiguration()
        {
            DoubleArray home = DoubleArray.From(new double[] { 0, -MathConstants.PIOverTwo, 0, -MathConstants.PIOverTwo, 0, 0 });
            return home;
        }

        /// <summary>
        /// Some default joint configurations that can be used as good IK seeds.
        /// These joint configurations were collected during our pick-and-place tasks when
        /// the arm was at poses that look natural to the human data collector.
        /// These typical joint configurations do not contain ALL potentially good
        /// joint configurations and are not expected to work with ALL pick-and-place
        /// tasks.  We have them here only for our current pick-and-place tests that
        /// we run every day.
        /// </summary>
        /// <returns>A list of good seed joint configurations</returns>
        public static List<double[]> GetUR5DefaultIKSeeds()
        {
            List<double[]> configList = new List<double[]>();

            // low horizontal poses
            configList.Add(new double[] { 0.698132, -1.754579, -1.925447, -2.635098, -1.506219, -0.063006 });
            configList.Add(new double[] { 1.396263, -1.754579, -1.925447, -2.635098, -1.506219, -0.063006 });
            configList.Add(new double[] { 2.0944, -1.754579, -1.925447, -2.635098, -1.506219, -0.063006 });

            // high horizontal poses
            configList.Add(new double[] { 0.174533, -1.929461, -1.578650, 0.398459, -4.681322, -0.061261 });
            configList.Add(new double[] { 1.2217, -1.929461, -1.578650, 0.398459, -4.681322, -0.061261 });
            configList.Add(new double[] { 2.2689, -1.929461, -1.578650, 0.398459, -4.681322, -0.061261 });

            // far vertical poses
            configList.Add(new double[] { 0.174533, -1.783901, -1.715310, -1.218240, -4.679402, -0.060563 });
            configList.Add(new double[] { 1.2217, -1.783901, -1.715310, -1.218240, -4.679402, -0.060563 });
            configList.Add(new double[] { 2.2689, -1.783901, -1.715310, -1.218240, -4.679402, -0.060563 });

            return configList;
        }

        /// <summary>
        /// A function that generates equivalent joint configurations based on the current joint configuration.
        /// End effector pose remains the same for all these equivalent joint configurations.
        /// This function is absolutely hardware kinematic specific.  Some arms may not have an
        /// equivalent generator like this.
        /// </summary>
        /// <param name="jointValues">Current joint configuration</param>
        /// <returns>Other joint values which are equivalent to the current one</returns>
        public static List<DoubleArray> UR5EquivalentJointConfigurationGenerator(DoubleArray jointValues)
        {
            List<DoubleArray> pool = new List<DoubleArray>();

            pool.Add(jointValues);

            // consider the joints' ranges are all [-360, 360]
            // let's loop through each joint individually and generate equivalent solution
            // by changing each of them
            for (int i = 0; i < jointValues.Count; ++i)
            {
                int numSolutionsSoFar = pool.Count;

                // only work on all the solutions we generated so far
                for (int j = 0; j < numSolutionsSoFar; ++j)
                {
                    DoubleArray newSolution = DoubleArray.From(pool[j]);
                    if (newSolution[i] > 0)
                    {
                        newSolution[i] -= MathConstants.TwoPI;
                    }
                    else
                    {
                        newSolution[i] += MathConstants.TwoPI;
                    }

                    pool.Add(newSolution);
                }
            }

            return pool;
        }
    }
}
